#include "System.h"

//16 IR Sensors will be read through ADP8
//
//Sensors will be externally muxed.  Port D0-D3 controls the mux channel


//Battery Voltage will be measured via ADP10.  



SensorData *PendingSensorData;
SensorData *CurrentSensorData;

SensorData SensorDataBuffer[2];

BYTE PendingSensorDataPtr;
BYTE CaptureSensorState;

volatile WORD   CurrentPotSetting = 0;

#define SENSOR_CAPTURE_STATE_IR_0         0
#define SENSOR_CAPTURE_STATE_IR_1         1
#define SENSOR_CAPTURE_STATE_IR_2         2
#define SENSOR_CAPTURE_STATE_IR_3         3
#define SENSOR_CAPTURE_STATE_IR_4         4
#define SENSOR_CAPTURE_STATE_IR_5         5
#define SENSOR_CAPTURE_STATE_IR_6          6
#define SENSOR_CAPTURE_STATE_IR_7          7
#define SENSOR_CAPTURE_STATE_IR_8          8
#define SENSOR_CAPTURE_STATE_IR_9          9
#define SENSOR_CAPTURE_STATE_IR_10        10
#define SENSOR_CAPTURE_STATE_IR_11        11
#define SENSOR_CAPTURE_STATE_IR_12        12
#define SENSOR_CAPTURE_STATE_IR_13        13
#define SENSOR_CAPTURE_STATE_IR_14        14
#define SENSOR_CAPTURE_STATE_IR_15        15
#define SENSOR_CAPTURE_STATE_ACCEL_X      16
#define SENSOR_CAPTURE_STATE_ACCEL_Y      17
#define SENSOR_CAPTURE_STATE_ACCEL_Z      18
#define SENSOR_CAPTURE_STATE_BATT         19
#define SENSOR_CAPTURE_STATE_POT          20


#define ADC_CH_IR       8
#define ADC_CH_BAT      10
#define ADC_CH_ACCEL_X  5
#define ADC_CH_ACCEL_Y  6
#define ADC_CH_ACCEL_Z  7
#define ADC_CH_POT      4

void InitSensorCapture()
{
    PendingSensorDataPtr = 0;
    
    (void)memset(&SensorDataBuffer[0],0,sizeof(SensorData));
    (void)memset(&SensorDataBuffer[1],0,sizeof(SensorData));
    
    FlopSensorDataBuffers();
    
    InitADC();
}


void FlopSensorDataBuffers()
{
   PendingSensorDataPtr++;
   
   PendingSensorDataPtr&=0x01;
   
   if(PendingSensorDataPtr == 0)
   {
      PendingSensorData = (SensorData *)&SensorDataBuffer[0];
      CurrentSensorData = (SensorData *)&SensorDataBuffer[1]; 
   } else
   {
      PendingSensorData = (SensorData *)&SensorDataBuffer[1];
      CurrentSensorData = (SensorData *)&SensorDataBuffer[0]; 
   }


}

void InitADC(void)
{
  
  //Reset Configuration Register 1
  //Note since we reset the configuration, the bit clears
  //aren't really needed in this init function but I 
  //Wanted to be clear in how the converter is initialized
  
  ADCCFG1 = 0;
  
  //Normal Power Mode
  ADCCFG1 &= ~(ADCCFG1_ADLPC_MASK);
  
  
  //Select the Bus clock
  ADCCFG1 &= ~(ADCCFG1_ADICLK0_MASK | ADCCFG1_ADICLK0_MASK);
  
  
  //Divide the input clock source by 8
  ADCCFG1 |= ADCCFG1_ADIV0_MASK | ADCCFG1_ADIV0_MASK;
 
 //Select 12-bit mode
 ADCCFG1 &=  ~(ADCCFG1_MODE0_MASK | ADCCFG1_MODE1_MASK);
  
  //Short Sample Time
 ADCCFG1 &=  ~ADCCFG1_ADLSMP_MASK;
  
  
 //Disable Asynchonous Clock output enable
 ADCCFG2 &=  ~ADCCFG2_ADACKEN_MASK;

 //Normal Conversion sequence speed
 ADCCFG2 &=  ~ADCCFG2_ADHSC_MASK;
 
 
 //Longest Sample Time
 ADCCFG2 &=  ~(ADCCFG2_ADLSTS0_MASK | ADCCFG2_ADLSTS1_MASK); 
 

//Select Software Triggers
ADCSC2 &= ~(ADCSC2_ADTRG_MASK);

//Disable the Compare function
ADCSC2 &= ~(ADCSC2_ACFE_MASK);


//Disable Continuous Conversions
ADCSC3 &= ~(ADCSC3_ADCO_MASK);


//Enable A/D channel on the physical Pin for the pot and Accelerometer
APCTL1 |= APCTL1_ADPC4_MASK | APCTL1_ADPC5_MASK | APCTL1_ADPC6_MASK | APCTL1_ADPC7_MASK;

//Enable the A/D Channel on the physical pin for the IR Sensors and battery voltage
APCTL2 |= APCTL2_ADPC8_MASK | APCTL2_ADPC10_MASK;  
  
}


void CaptureSensors()
{
    //A write to the control register starts a conversion
    //Start with IR sensor and mark sure interrupts are enables
    CaptureSensorState = SENSOR_CAPTURE_STATE_IR_0;
    //Start the IR Mux at zero
    PTDD = CaptureSensorState;
    ADCSC1 = ADC_CH_IR | ADCSC1_AIEN_MASK;
}


interrupt VectorNumber_Vadc  void ADC_Complete(void)
{


  switch(CaptureSensorState)
  {
    default:
      CaptureSensorState = 0;
    case SENSOR_CAPTURE_STATE_IR_0:        
    case SENSOR_CAPTURE_STATE_IR_1:         
    case SENSOR_CAPTURE_STATE_IR_2:         
    case SENSOR_CAPTURE_STATE_IR_3:        
    case SENSOR_CAPTURE_STATE_IR_4:         
    case SENSOR_CAPTURE_STATE_IR_5:        
    case SENSOR_CAPTURE_STATE_IR_6:        
    case SENSOR_CAPTURE_STATE_IR_7:         
    case SENSOR_CAPTURE_STATE_IR_8:         
    case SENSOR_CAPTURE_STATE_IR_9:        
    case SENSOR_CAPTURE_STATE_IR_10:       
    case SENSOR_CAPTURE_STATE_IR_11:       
    case SENSOR_CAPTURE_STATE_IR_12:       
    case SENSOR_CAPTURE_STATE_IR_13:        
    case SENSOR_CAPTURE_STATE_IR_14:       
    case SENSOR_CAPTURE_STATE_IR_15:     
          
          //Grab the Data
          PendingSensorData->IR[CaptureSensorState] =  ADCRL;        
          CaptureSensorState++;
          //Set the extern Mux to the next channel for the IRs
          PTDD = CaptureSensorState;
          //if we are on the last IR sensor, start the next conversion at the x accel.
          //if not continue with the IR
          if( CaptureSensorState == SENSOR_CAPTURE_STATE_ACCEL_X)
            ADCSC1 = ADC_CH_ACCEL_X  | ADCSC1_AIEN_MASK;
          else
            ADCSC1 = ADC_CH_IR | ADCSC1_AIEN_MASK;
          
    break;
    
    case SENSOR_CAPTURE_STATE_ACCEL_X:
    
     PendingSensorData->Accel[0] =  ADCRL;        
     CaptureSensorState++;
     ADCSC1 = ADC_CH_ACCEL_Y | ADCSC1_AIEN_MASK;
    
    break;
    
    case SENSOR_CAPTURE_STATE_ACCEL_Y:
    
     PendingSensorData->Accel[1] =  ADCRL;        
     CaptureSensorState++;
     ADCSC1 = ADC_CH_ACCEL_Z | ADCSC1_AIEN_MASK;
    
    break;
    
    case SENSOR_CAPTURE_STATE_ACCEL_Z:
     PendingSensorData->Accel[2] =  ADCRL;        
     CaptureSensorState++;
     ADCSC1 = ADC_CH_BAT | ADCSC1_AIEN_MASK;
    break;
    
    case SENSOR_CAPTURE_STATE_BATT:
     PendingSensorData->Batt =  ADCRL;        
     CaptureSensorState++;
     ADCSC1 = ADC_CH_POT | ADCSC1_AIEN_MASK;
    break;
        
    case SENSOR_CAPTURE_STATE_POT:
      PendingSensorData->Pot =  ADCRL;    
      PendingSensorData->RPM =  CurrentRPMMeasurement.FullValue; 
     //Now we are all done. Flop the buffers
     //THis will start again when CaptureSensors() is called again
      FlopSensorDataBuffers();
    break;         
    
  }
}